#!/usr/bin/env python  3
  
import rospy  
from sensor_msgs.msg import Image  
from cv_bridge import CvBridge  
import cv2  
import os  
  
def publish_image_from_file(file_path, topic_name, rate):  
    rospy.init_node('image_publisher', anonymous=True)  
    image_pub = rospy.Publisher(topic_name, Image, queue_size=10)  
    bridge = CvBridge()  
    rate = rospy.Rate(rate)  
  
    while not rospy.is_shutdown():  
        if os.path.exists(file_path):  
            img = cv2.imread(file_path)  
            if img is not None:  
                try:  
                    ros_image = bridge.cv2_to_imgmsg(img, "bgr8")  
                except CvBridgeError as e:  
                    print(e)  
                    return  
                image_pub.publish(ros_image)  
                rate.sleep()  
            else:  
                rospy.logerr("Failed to load image from: {}".format(file_path))  
        else:  
            rospy.logerr("Image file does not exist: {}".format(file_path))  
  
if __name__ == '__main__':  

    file_path = rospy.get_param('~image_path', default='/home/orangepi/Documents/test/1.jpg')  
    topic_name = rospy.get_param('~topic_name', default='/TopoCam/img')  
    rate_hz = rospy.get_param('~rate', default=1.0)  

  
    publish_image_from_file(file_path, topic_name, rate_hz)
